<!DOCTYPE html>

<html>

  <head>
    <title>Robotic Manipulation: Basic Pick and Place</title>
    <meta name="Robotic Manipulation: Basic Pick and Place" content="text/html; charset=utf-8;" />
    <link rel="canonical" href="http://manipulation.csail.mit.edu/pick.html" />

    <script src="https://hypothes.is/embed.js" async></script>
    <script type="text/javascript" src="htmlbook/book.js"></script>

    <script src="htmlbook/mathjax-config.js" defer></script> 
    <script type="text/javascript" id="MathJax-script" defer
      src="https://cdn.jsdelivr.net/npm/mathjax@3/es5/tex-chtml.js">
    </script>
    <script>window.MathJax || document.write('<script type="text/javascript" src="htmlbook/MathJax/es5/tex-chtml.js" defer><\/script>')</script>

    <link rel="stylesheet" href="htmlbook/highlight/styles/default.css">
    <script src="htmlbook/highlight/highlight.pack.js"></script> <!-- http://highlightjs.readthedocs.io/en/latest/css-classes-reference.html#language-names-and-aliases -->
    <script>hljs.initHighlightingOnLoad();</script>

    <link rel="stylesheet" type="text/css" href="htmlbook/book.css" />
  </head>

<body onload="loadChapter('manipulation');">

<div data-type="titlepage">
  <header>
    <h1><a href="index.html" style="text-decoration:none;">Robotic Manipulation</a></h1>
    <p data-type="subtitle">Perception, Planning, and Control</p> 
    <p style="font-size: 18px;"><a href="http://people.csail.mit.edu/russt/">Russ Tedrake</a></p>
    <p style="font-size: 14px; text-align: right;"> 
      &copy; Russ Tedrake, 2020<br/>
      Last modified <span id="last_modified"></span>.</br>
      <script>
      var d = new Date(document.lastModified);
      document.getElementById("last_modified").innerHTML = d.getFullYear() + "-" + (d.getMonth()+1) + "-" + d.getDate();</script>
      <a href="misc.html">How to cite these notes, use annotations, and give feedback.</a><br/>
    </p>
  </header>
</div>

<p><b>Note:</b> These are working notes used for <a
href="http://manipulation.csail.mit.edu/Fall2020/">a course being taught
at MIT</a>. They will be updated throughout the Fall 2020 semester.  <!-- <a 
href="https://www.youtube.com/channel/UChfUOAhz7ynELF-s_1LPpWg">Lecture  videos are available on YouTube</a>.--></p> 

<table style="width:100%;"><tr style="width:100%">
  <td style="width:33%;text-align:left;"><a class="previous_chapter" href=robot.html>Previous Chapter</a></td>
  <td style="width:33%;text-align:center;"><a href=index.html>Table of contents</a></td>
  <td style="width:33%;text-align:right;"><a class="next_chapter" href=pose.html>Next Chapter</a></td>
</tr></table>


<!-- EVERYTHING ABOVE THIS LINE IS OVERWRITTEN BY THE INSTALL SCRIPT -->
<chapter style="counter-reset: chapter 2"><h1>Basic Pick and Place</h1>
  <a style="float:right; margin-top:-80px;" target="pick" href="https://colab.research.google.com/github/RussTedrake/manipulation/blob/master/pick.ipynb"><img src="https://colab.research.google.com/assets/colab-badge.svg" alt="Open Corresponding Notebook In Colab"/></a>
  <div style="clear:right;"></div>

  <figure>
    <img src="figures/pick.png" style="width:70%"/>
    <figcaption>Your challenge: command the robot to pick up the brick and place
    it in a desired position/orientation.</figcaption>
    <todo>Update this image once we get a better schunk model.</todo>
  </figure>

  <p>The stage is set.  You have your robot.  I have a little red foam brick.
  I'm going to put it on the table in front of your robot, and your goal is to
  move it to a desired position/orientation on the table.  I want to defer the
  <i>perception</i> problem for one chapter, and will let you assume that you
  have access to a perfect measurement of the current position/orientation of
  the brick.  Even without perception, completing this task requires us to
  build up a basic toolkit for geometry and kinematics; it's a natural place to
  start.</p>

  <p>First, we will establish some terminology and notation for kinematics. This
  is one area where careful notation can yield dividends, and sloppy notation
  will inevitably lead to confusion and bugs.  The Drake developers have gone to
  great length to establish and document a consistent <a
  href="https://drake.mit.edu/doxygen_cxx/group__multibody__notation.html">
  multibody notation</a>, which we call "Monogram Notation".  The documentation
  even includes some of the motivation/philosophy behind that notation. I'll use
  the monogram notation throughout this text.</p>
  
  <p>If you'd like a more extensive background on kinematics than what I provide
  here, my favorite reference is still <elib>Craig05</elib>.  For free online
  resources, Chapters 2 and 3 of the 1994 book by Murray et al. (now free
  online)<elib>Murray94</elib> are also excellent, as are the first seven
  chapters of <a
  href="http://hades.mech.northwestern.edu/index.php/Modern_Robotics">Modern
  Robotics</a> by Lynch and Park<elib>Lynch17</elib> (they also have excellent
  accompanying videos).  Unfortunately, with three different references you'll
  get three (slightly) different notations; ours is most similar to
  <elib>Craig05</elib>.</p>
    
  <p>Please don't get overwhelmed by how much background material there is to
  know!  I am personally of the opinion that a clear understanding of just a few
  basic ideas should make you very effective here.  The details will come
  later, if you need them.</p>    

  <section><h1>Monogram Notation</h1>

    <p><i>The following concepts are disarmingly subtle. I've seen incredibly
    smart people assume they knew them and then perpetually stumble over
    notation.  I did it for years myself.  Take a minute to read this
    carefully!</i></p>

    <p>Perhaps the most fundamental concept in geometry is the concept of a
    point.  Points occupy a position in space, and they can have names, e.g.
    point $A$, $C$, or more descriptive names like $B_{cm}$ for the center of
    mass of body $B$. We'll denote the position of the point by using a position
    vector $p^A$; that's $p$ for position, and not for point, because other
    geometric quantities can also have a position.</p>

    <p>But let's be more careful.  Position is actually a relative quantity.
    Really, we should only ever write the position of two points relative to
    each other. We'll use e.g. $^Ap^C$ to denote the position of $C$ relative to
    $A$.  The left superscript looks mighty strange, but we'll see that it pays
    off once we start transforming points.</p>
    
    <p>Every time we describe the (relative) position as a vector of numbers, we
    need to be explicit about the frame we are using, specifically
    the "expressed-in" frame. All of our frames are defined by orthogonal unit
    vectors that follow the "right-hand rule". We'll give a frame a name, too,
    like $F$.  If I want to write the position of point $C$ relative to point
    $A$, expressed in frame $F$, I will write $^Ap^C_F$. If I ever want to get just a single component of that vector, e.g. the $x$ component, then I'll use $^Ap^C_{F_x}$.</p>
    
    <p>That is seriously heavy notation.  I don't love it myself, but it's
    the most durable I've got, and we'll have shorthand for when the context is
    clear.</p>

    <p><img style="height:90px;float:left;margin-right:10px"
    src="figures/frames.png"/> There are a few very special frames.
    We use $W$ to denote the <i>world frame</i>.  We think about the world frame
    in Drake using vehicle coordinates (positive $x$ to the front, positive $y$
    to the <i>left</i>, and positive $z$ is up).  The other particularly special
    frames are the <i>body frames</i>: every body in the multibody system has a
    unique frame attached to it.  We'll typically use $B_i$ to denote the frame
    for body $i$.</p>

    <p>Frames have a position, too -- it coincides with the frame origin.  So it
    is perfectly valid to write $^Wp^A_W$ to denote the position of point $A$
    relative to the origin of the world frame, expressed in the world frame.
    Here is where the shorthand comes in.  If the position of a quantity is
    relative to a frame, and expressed in the same frame, then we can safely
    omit the subscript.  $^Fp^A \equiv {^Fp^A_F}$. Furthermore,
    if the "relative to" field is omitted, then we assume that the point is
    relative to $W$, so $p^A \equiv ^Wp^A_W$.</p>

    <figure>
      <img style="width:60%" src="figures/notation.svg"></img>
      <!-- Note: I made this in slides.com, exported to pdf, cropped in acrobat,
      then converted with pdf2svg. -->
    </figure>

    <p>Frames also have an orientation.  We'll use $R$ to denote a
    <i>rotation</i>, and follow the same notation, writing $^BR^A$ to denote the
    rotation of frame $A$ relative to frame $B$.  Unlike vectors, pure rotations
    do not have an additional "expressed in" frame.</p>

    <p>A frame $F$ can be specified completely by a position and rotation
    relative to another frame.  Taken together, we call the position and
    rotation a <a
    href="https://drake.mit.edu/doxygen_cxx/group__multibody__spatial__pose.html">
    <i>spatial pose</i></a>, or just <i>pose</i>.  A <i>spatial transform</i>,
    or just <i>transform</i>, is the "verb form" of pose.  In Drake we use <a
    href="https://drake.mit.edu/doxygen_cxx/classdrake_1_1math_1_1_rigid_transform.html"><code>RigidTransform</code></a>
    to represent a pose/transform, and denote it with the letter $X$. $^BX^A$ is
    the pose of frame $A$ relative to frame $B$.  When we talk about the pose of
    an object $O$, without mentioning a reference frame explicitly, we mean
    $^WX^O$ where $O$ is the body frame of the object.</p>
    
    <p>The Drake <a
    href="https://drake.mit.edu/doxygen_cxx/group__multibody__notation.html">
    documentation</a> also discusses how to use this notation in code. In short,
    $^Bp^A_C$ is written <code>p_BA_C</code>. It works, I promise.</p>

    <!--
    <subsection><h1>Rotation representations</h1>
    
      <p>There is some subtly on how we choose to represent a relative pose /
      rigid transform, due the various ways that we can represent a rotation.</p>

      <p>Quaternions, Euler angles, Axis angles, Rotation matrices.</p>
      <p>We'll get into them if/when we need to, but for now it's sufficient to
      understand that `RigidTransform`...</p>

      <p>Everything is better in 2D.  We can use a scalar rotation angle to
      completely specify a rotation represented in a frame, and we can convert
      seamlessly between this scalar and the 2D rotation matrices.  I'll try to
      use 2D examples to demonstrate core concepts whenever possible.</p>

    </subsection>
    <example><h1>Pose in 2D</h1>
      <todo>Colab example with sliders for x,z,theta.  Mustard bottle?</todo>
    
    </example>

    <example><h1>Pose in 3D</h1>
      <todo>Colab example with pose sliders.  Mustard bottle?  But also output, at least in text, the other representations.</todo>
    </example>
-->

  </section>

  <section><h1>Pick and place via spatial transforms</h1>
  
    <p>Now that we have the notation, we can formulate our approach to the basic
    pick and place problem.  Let us call our object, $O$, and our gripper, $G$.
    Our idealized perception sensor tells us $^WX^O$.  Let's create a frame
    $O_d$ to describe the "desired" pose of the object, $^WX^{O_d}$.
    So pick and place manipulation is simply trying to make $X^O = X^{O_d}$.</p>
    
    <todo>Add a figure here (after Terry's PR lands).</todo>

    <div>To accomplish this, we will assume that the object doesn't move
    relative to the world ($^WX^O$ is constant) when the gripper is open, and
    the object doesn't move relative to the gripper ($^GX^O$ is constant) when
    the gripper is closed.  Then we can:
      <ul style="margin:0"><li>move the gripper in the world, $X^G$, to an
      appropriate pose relative to the object: $^OX^{G_{grasp}}$.</li>
      <li>close the gripper.</li>
      <li>move the gripper+object to the desired pose, $X^O = X^{O_d}$.</li>
      <li>open the gripper, and retract the hand.</li>
      </ul>
    To simplify the problem of approaching the object (without colliding with
    it) and retracting from the object, we will insert a "pregrasp pose",
    $^OX^{G_{pregrasp}}$, above the object as an intermediate step.
    </div>

    <p>Clearly, programming this strategy requires good tools for working with
    these transforms, and for relating the pose of the gripper to the joint
    angles of the robot.</p>
  </section>

  <section><h1>Spatial Algebra</h1>
  
    <p>Here is where we start to see the pay-off from our heavy notation, as we
    define the rules of converting positions, rotations, poses, etc. between
    different frames.  Without the notation, this invariably involves
    me with my right hand in the air making the "right-hand rule", and my head
    twisting around in space.  With the notation, it's a simple matter of lining
    up the symbols properly, and we're more likely to get the right answer!</p>
  
    <div>
    Here are the basic rules of algebra for our spatial quantities:  
    <ul>
      <li>Positions expressed in the same frame can be added when their
      reference and target symbols match: \begin{equation}{}^Ap^B_F + {}^Bp^C_F
      = {}^Ap^C_F.\end{equation} The additive inverse is well defined:
      \begin{equation}{}^Ap^B_F = - {}^Bp^A_F.\end{equation} Those should be
      pretty intuitive; make sure you confirm them for yourself.</li>
      <li>Multiplication by a rotation can be used to change the "expressed in"
      frame: \begin{equation}{}^Ap^B_G = {}^GR^F {}^Ap^B_F.\end{equation}  You
      might be surprised that a rotation alone is enough to change the
      expressed-in frame, but it's true. The position of the expressed-in frame
      does <i>not</i> affect the relative position between two points.</li>
      <li>Rotations can be multiplied when their reference and target symbols
      match: \begin{equation}{}^AR^B \: {}^BR^C = {}^AR^C.\end{equation}  The
      inverse operation is also simply defined:
      \begin{equation}\left[{}^AR^B\right]^{-1} = {}^BR^A.\end{equation} When
      the rotation is represented as a rotation matrix, this is literally the
      matrix inverse, and since rotation matrices are orthonormal, we also have
      $R^{-1}=R^T.$</li>
      <li>Transforms bundle this up into a single, convenient notation when
      positions are relative to a frame (and the same frame they are expressed
      in): \begin{equation}{}^Gp^A = {}^GX^F {}^Fp^A = {}^Gp^F + {}^Fp^A_G
      = {}^Gp^F + {}^GR^F {}^Fp^A.\end{equation}</li>
      <li>Transforms compose: \begin{equation}{}^AX^B {}^BX^C =
      {}^AX^C,\end{equation}  and have an inverse
      \begin{equation}\left[{}^AX^B\right]^{-1} = {}^BX^A.\end{equation}  Please
      note that for transforms, we generally do
      <i>not</i> have that $X^{-1}$ is $X^T,$ though it still has a simple form.
    </li>
    </ul></div>

    <p>In practice, transforms are implemented using <a
    href="https://drake.mit.edu/doxygen_cxx/group__multibody__spatial__pose.html">homogenous
    coordinates</a>, but for now I'm happy to leave that as an implementation
    detail.</p>

    <example><h1>From camera frame to world frame</h1>
      
      <todo>Add a figure here.</todo>

      <p>Imagine that I have a depth camera mounted in a fixed pose in my
      workspace.  Let's call the camera frame $C$ and denote its pose in the
      world with ${}^WX^C$.  This pose is often called the camera
      "extrinsics".</p>

      <p>A depth camera returns points in the camera frame.  Therefore,
      we'll write this position of point $P_i$ with ${}^Cp^{P_i}$.  If we want
      to convert the point into the world frame, we simply have $$p^{P_i}
      = X^C {}^Cp^{P_i}.$$</p>

      <p>This is a work-horse operation for us.  We often aim to merge points
      from multiple cameras (typically in the world frame), and always need to
      somehow relate the frames of the camera with the frames of the robot.</p>
    
    </example>

  </section>

  <section id="kinematics"><h1>Forward kinematics</h1>
  
    <p>The spatial algebra gets us pretty close to what we need for our pick and
    place algorithm.  But remember that the interface we have with the robot
    reports measured joint positions, and expects commands in the form of joint
    positions.  So our remaining task is to convert between joint angles and
    cartesian frames.  We'll do this in steps, the first step is to go from
    joint positions to cartesian frames: this is known as <i>forward
    kinematics</i>.

    <p>Throughout this text, we will refer to the joint positions of the robot
    (also known as "configuration" of the robot) using a vector $q$.  If the
    configuration of the scene includes objects in the environment as well as
    the robot, we would use $q$ for the entire configuration vector, and use
    e.g. $q_{robot}$ for the subset of the vector corresponding to the robot's
    joint positions. Therefore, the goal of forward kinematics is to produce a
    map: \begin{equation}X^G = f_{kin}^G(q).\end{equation}  Moreover, we'd like
    to have forward kinematics available for any frame we have defined in the
    scene.  Our spatial notation and spatial algebra makes this computation
    relatively straight-forward.</p>

    <subsection id="kinematic_tree"><h1>The kinematic tree</h1>

      <p>In order to facilitate kinematics and related multibody computations,
      the <code>MultibodyPlant</code> organizes all of the bodies in the world
      into a tree topology.  Every body (except the world body) has a parent,
      which it is connected to via either a <code>Joint</code> or a "floating
      base".</p>

      <example><h1>Inspecting the kinematic tree</h1>

        <p>Drake provides some visualization support for inspecting the
        kinematic tree data structure.  The kinematic tree for an iiwa is more
        of a vine than a tree (it's a serial manipulator), but the tree for the
        dexterous hands are more interesting.  I've added our brick to the
        example, too, so that you can see that a "free" body is just another
        branch off the world root node.</p>

        <p><a target="pick"
          href="https://colab.research.google.com/github/RussTedrake/manipulation/blob/master/pick.ipynb#scrollTo=ILYLouFTjv6e"><img
          src="https://colab.research.google.com/assets/colab-badge.svg" alt="Open In Colab"/></a>
          </p>
    
        <todo>Insert topology visualization here (once it is better)</todo>
      </example>

      <p>Every <code>Joint</code> and "floating base" has some number of
      position variables associated with it -- a subset of the configuration
      vector $q$ -- and knows how to compute the configuration dependent
      transform across the joint from the child joint frame $J_C$ to the parent
      joint frame $J_P$: ${}^{J_P}X^{J_C}(q)$.  Additionally, the kinematic tree
      defines the (fixed) transforms from the joint frame to the child body
      frame, ${}^CX^{J_C}$, and from the joint frame to the parent frame,
      ${}^PX^{J_P}$. Altogether, we can compute the configuration transform
      between any one body and its parent, $${}^PX^C(q) = {}^PX^{J_P}
      {}^{J_P}X^{J_C}(q) {}^{J_C}X^C.$$</p>

      <example><h1>Specifying the kinematic tree in URDF</h1></example>

      <example><h1>Specifying the kinematic tree in SDF</h1></example>

      <p>You might be tempted to think that every time you add a joint to the
      <code>MultibodyPlant</code>, you are adding a degree of freedom.  But it
      actually works the other way around.  Every time you add a <i>body</i>
      to the plant, you are adding many degrees of freedom.  But you can then
      add joints to <i>remove</i> those degrees of freedom; joints are
      constraints.  "Welding" the robot's base to the world frame removes all of
      the floating degrees of freedom of the base.  Adding a rotational joint
      between a child body and a parent body removes all but one degree of
      freedom, etc.</p>

      <p>Interestingly, <code>SceneGraph</code> (the geometry engine) does not
      maintain any tree structure.  All that <code>SceneGraph</code> knows about
      is a list of frames specified relative to the world frame, $W$, and a list
      of geometries attached to each frame.</p> 
    
    </subsection>

    <subsection><h1>Forward kinematics for pick and place</h1>
    
      <p>In order to compute the pose of the gripper in the world, $X^G$, we
      simply query the parent of the gripper frame in the kinematic tree, and
      recursively compose the transforms until we get to the world frame.</p>

      <figure id="kinematic_frames">
        <img style="width:40%;margin-right:40px"
        src="figures/kinematic_frames_iiwa.png"/><img style="width:40%"
        src="figures/kinematic_frames_gripper.png"/>
        <figcaption>Kinematic frames on the iiwa (left) and the WSG (right). For
        each frame, the <span style="color:red">positive $x$ axis is in
        red</span>, the <span style="color:green">positive $y$ axis is in
        green</span>, and the <span style="color:blue">positive $z$ axis is in
        blue</span>.  It's (hopefully) easy to remember: XYZ $\Leftrightarrow$
        RGB.</figcaption>
      </figure>

      <example><h1>Forward kinematics for the gripper frame</h1>
      
        <p>Let's evaluate the pose of the gripper in the world frame: $X^G$.  We
        know that it will be a function of configuration of the robot, which is
        just a part of the total state of the <code>MultibodyPlant</code>.  The
        following example shows you how it works.</p>

        <p><a target="pick"
          href="https://colab.research.google.com/github/RussTedrake/manipulation/blob/master/pick.ipynb#scrollTo=5SjOClhTltPk"><img
          src="https://colab.research.google.com/assets/colab-badge.svg" alt="Open In Colab"/></a>
          </p>

        <p>The key lines are 
          <code class="language-python"><pre>
gripper = plant.GetBodyByName("body")
pose = plant.EvalBodyPoseInWorld(context, gripper)</pre></code>
        Behind the scenes, the <code>MultibodyPlant</code> is doing all of the
        spatial algebra we described above to return the pose (and also some
        clever caching because you can reuse much of the computation when you
        want to evaluate the pose of another frame on the same robot).  
        </p>

      </example>

      <example><h1>Forward kinematics of "floating-base" objects</h1>
      
        <p>Consider the special case of having a <code>MultibodyPlant</code>
        with exactly one body added, and no joints.  The kinematic tree is
        simply the world frame, the body frame, and they are connected by the
        "floating base".  What does the forward kinematics function: $$X^B =
        f_{kin}^B(q),$$ look like in that case?  If $q$ is already representing
        the floating-base configuration, is $f^B_{kin}$ just the identity
        function?</p>

        <p>This gets into the subtle points of how we represent transforms, and
        how we represent rotations in particular.  There are many possible
        representations of 3D rotations, they are good for different things, and
        unfortunately, there is no one representation to rule them all. (This is
        one of the many reasons why everything is better in 2D!)  Common
        representations include 3x3 rotation matrices, Euler angles (e.g.
        Roll-Pitch-Yaw), axis angle, and unit quaternions. In Drake, we provide
        all of these representations, and make it easy to convert back and forth
        between them.  In order to make the spatial algebra efficient, we use
        rotation matrices in our <code>RigidTransform</code>, but in order to
        have a more compact representation of configuration we use unit
        quaternions in our configuration vector, $q$.  You can think of unit
        quaternions as a form of axis angles that have been carefully normalized
        to be unit length and have magical properties. My favorite careful
        description of quaternions is probably chapter 1 of
        <elib>Stillwell08</elib>.</p>

        <p>As a result, for this example, the software implementation of the
        function $f_{kin}^B$ is precisely the function that converts the
        position $\times$ unit quaternion representation into the position
        $\times$ rotation matrix representation.</p>
      </example>

    </subsection>

  </section>

  <section id="jacobian"><h1>Differential kinematics (Jacobians)</h1>

    <p>The forward kinematics machinery gives us the ability to compute the pose
    of the gripper and the pose of the object, both in the world frame.  But
    if our goal is to <i>move</i> the gripper to the object, then we should
    understand how changes in the joint angles relate to changes in the gripper
    pose.  This is traditionally referred to as "differential kinematics".</p>

    <p>At first blush, this is straightforward.  The change in pose is related
    to a change in joint positions by the (partial) derivative of the forward
    kinematics:  \begin{equation}dX^B = \pd{f_{kin}^B(q)}{q} dq = J^B(q)dq.
    \label{eq:jacobian}\end{equation} Partial derivatives of a function are
    referred to as "Jacobians" in many fields; in robotics it's rare to refer to
    derivatives of the kinematics as anything else.</p>

    <div>All of the subtlety, again, comes in because of the multiple
    representations that we have for 3D rotations (rotation matrix, unit
    quaternions, ...).  While there is no one best representation for 3D
    rotations, it <i>is</i> possible to have one canonical representation for
    differential rotations.  Therefore, without any loss of generality, we can
    represent the rate of change in pose using a six-component vector for <a
    href="https://drake.mit.edu/doxygen_cxx/group__multibody__spatial__vectors.html"><i>spatial
    velocity</i></a>: \begin{equation}{}^AV^B_C = \begin{bmatrix} {}^A\omega^B_C
    \\ {}^A\text{v}^B_C \end{bmatrix}.\end{equation} ${}^AV^B_C$ is the spatial
    velocity of frame $B$ relative to frame $A$ expressed in frame $C$,
    ${}^A\omega^B_C \in \Re^3$ is the <i>angular velocity</i> (of frame $B$
    relative frame $A$ expressed in frame $C$), and ${}^A\text{v}^B_C \in \Re^3$
    is the <i>translational velocity</i> (along with the same shorthands as for
    positions).  Translational and rotational velocities are simply vectors, so
    they can be operated on just like positions:<ul>
      <li>Spatial velocities add (when the frames match): \begin{equation}
      {}^A\text{v}^B_F + {}^B\text{v}^C_F = {}^A\text{v}^C_F, \qquad
      {}^A\omega^B_F + {}^B\omega^C_F = {}^A\omega^C_F,\end{equation} and have
      the additive inverse. For the translational velocity, the addition is perhaps known from high-school physics; for the angular velocity I consider it quite surprising, and <a href="#angular_velocity_addition"></a>deserves to be verified</a>.</li>
      <li>Rotation matrices can be used to change between the "expressed-in"
      frames <b>when those frames are fixed</b>: \begin{equation}
      {}^A\text{v}^B_G = {}^GR^F {}^A\text{v}^B_F, \qquad {}^A\omega^B_G =
      {}^GR^F {}^A\omega^B_F, \qquad \text{when } {}^G\dot{R}^F=0.\end{equation}
      </li>
      <li><details><summary>If the relative pose between two frames is not
      fixed, then we get slightly more complicated formulas involving vector
      cross products; they are derived from the derivative of the transform
      equation and the chain rule.  (Click the triangle to expand those details,
      but consider skipping them for now!)</summary>  The angular velocity
      vector is related to the time-derivative of a rotation matrix via the
      skew-symmetric (as verified by differentiating $RR^T=I$) matrix:
      \begin{equation} \dot{R}R^T = \dot{R}R^{-1} = \begin{bmatrix} 0 &
      -\omega_z & \omega_y \\ \omega_z & 0 & -\omega_x \\ -\omega_y & \omega_x &
      0 \end{bmatrix}.\end{equation} Multiplying a vector by this matrix can be
      written as a vector-cross product: \begin{equation} \dot{R}R^T p =
      \dot{R}R^{-1} p = \omega \times p.\end{equation}  Therefore,
      differentiating $${}^Gp^A = {}^GX^F {}^Fp^A = {}^Gp^F + {}^GR^F
      {}^Fp^A,$$ yields \begin{align} {}^G\text{v}^A =& {}^G\text{v}^F +
      {}^G\dot{R}^F {}^Fp^A + {}^GR^F {}^F\text{v}^A \nonumber \\ =&
      {}^G\text{v}^F + {}^G\omega^F \times {}^GR^F {}^Fp^A + {}^GR^F
      {}^F\text{v}^A.\end{align}  <!--Similarly, differentiating ${}^AR^C =
      {}^AR^B {}^BR^C$ yields--></details></li>
    </ul>
    </div>


    <!--
    <p>Why is it that we can get away with just three components for spatial
    velocity, but not for rotations?  In short, this is because a three 
    component vector has both a direction (which we use to denote the axis) and 
    a magnitude.  Using the magnitude to denote an angle is degenerate because 
    our representation should be periodic in $2\pi$, but using them for angular
    velocities doesn't have this problem. Using sine of the angle resolves it,
    but we also need to store the cosine; that's <a
    href="https://en.wikipedia.org/w/index.php?title=Quaternions_and_spatial_rotation&section=14#The_hypersphere_of_rotations">precisely
    what happens</a> in the unit quaternions!</p>  -->

    <p>There is one more velocity to be aware of: I'll use $v$ to denote the
    generalized velocity vector of the plant.  While a spatial velocity $^AV^B$
    is six components, a translational or angular velocity, $^B\text{v}^C$ or
    $^B\omega^C$, is three components, the generalized velocity vector is whatever
    size it needs to be to encode the time derivatives of the configuration
    variables, $q$.  For the iiwa welded to the world frame, that means it has
    seven components.  I've tried to be careful to typeset each of these v's
    differently throughout the notes. Almost always the distinction is also
    clear from the context.</p>

    <example><h1>Don't assume $\dot{q} \equiv v$</h1>
    
      <p>The unit quaternion representation is four components, but these must
      form a "unit vector" of length 1.  Rotation matrices are 9 components, but
      they must form an orthonormal matrix with $\det(R)=1$.  It's pretty great
      that for <i>changes</i> in rotation, we can use an <i>unconstrained</i>
      three component vector, what we've called the angular velocity vector,
      $\omega$.  And you really should use it; getting rid of that constraint
      makes both the math and the numerics better.</p>

      <p>But there is one minor nuisance that this causes.  We tend to want to
      think of the generalized velocity as the time derivative of the
      generalized positions.  This works when we have only our iiwa in the
      model, and it is welded to the world frame.  But we cannot assume this in
      general; not when floating-base rotations are concerned.  As evidence,
      here is a simple example that loads exactly one rigid body into the
      <code>MultibodyPlant</code>, and then prints its
      <code>Context</code>.</p>

      <p><a target="pick"
        href="https://colab.research.google.com/github/RussTedrake/manipulation/blob/master/pick.ipynb#scrollTo=8ZUhwdTOtOjp"><img
        src="https://colab.research.google.com/assets/colab-badge.svg" alt="Open In Colab"/></a>
        </p>

      <p>The output looks like this:
      <code><pre>Context
--------
Time: 0
States:
  13 continuous states
    1 0 0 0 0 0 0 0 0 0 0 0 0

plant.num_positions() = 7
plant.num_velocities() = 6</pre></code></p>

      <p>You can see that this system has 13 total state variables.  7 of them
      are positions, $q$; we use unit quaternions in the position vector.  But
      we have just 6 velocities, $v$; we use angular velocities in the the
      velocity vector.  Clearly, if the length of the vectors don't even match,
      we do <i>not</i> have $\dot{q} = v$.</p>

      <p>It's not really any harder; Drake provides the
      <code>MultibodyPlant</code> methods <code>MapQDotToVelocity</code> and
      <code>MapVelocityToQDot</code> to go back and forth between them.  But you
      have to remember to use them!</p>

    </example>

    <p>Due to the multiple possible representations of 3D rotation, and the
    potential difference between $\dot{q}$ and $v$, there are actually
    <b>many</b> different kinematic Jacobians possible.  You may hear the terms
    "analytic Jacobian", which refers to the explicit partial derivative of the
    forward kinematics (as written in \eqref{eq:jacobian}), and "geometric
    Jacobian" which replaces 3D rotations on the left-hand side with spatial
    velocities.  In Drake's <code>MultibodyPlant</code>, we currently offer the geometric Jacobian versions via
    <ul>
      <li> <code>CalcJacobianAngularVelocity</code>,</li>
      <li><code>CalcJacobianTranslationalVelocity</code>, and</li>
      <li><code>CalcJacobianSpatialVelocity</code>,</li>
    </ul> with each taking an argument to specify whether you'd like the
    Jacobian with respect to $\dot{q}$ or $v$. If you really like the
    analytical Jacobian, you could get it (much less efficiently) using our
    support for automatic differentiation.</p>

    <example><h1>Kinematic Jacobians for pick and place</h1>
    
      <p>Let's repeat the setup from above, but we'll print out the Jacobian of the gripper frame, relative to the world frame, expressed in the world frame.</p>

      <p><a target="pick"
        href="https://colab.research.google.com/github/RussTedrake/manipulation/blob/master/pick.ipynb#scrollTo=hSeD3PBotJUU"><img
        src="https://colab.research.google.com/assets/colab-badge.svg" alt="Open In Colab"/></a>
        </p>

    </example>

  </section>

  <section><h1>Differential inverse kinematics</h1>
    
    <p>There is important structure in Eq \eqref{eq:jacobian}.  Make sure you
    didn't miss it.  The relationship between joint velocities and end-effector
    velocities is (configuration-dependent) linear: \begin{equation}V^G =
    J^G(q)v.\end{equation}  Maybe this gives us what we need to produce changes
    in gripper frame $G$?  If I have a desired gripper frame velocity $V^{G_d}$,
    then how about commanding a joint velocity $v = \left[J^G(q)\right]^{-1}
    V^{G_d}$?</p>

    <subsection><h1>The Jacobian pseudo-inverse</h1>

      <p>Any time you write a matrix inverse, it's important to check that the
      matrix is actually invertible.  As a first sanity check: what are the
      dimensions of $J^G(q)$?  We know the spatial velocity has six components.
      Our gripper frame is welded directly on the last link of the iiwa, and the
      iiwa has seven positions, so we have $J^G(q_{iiwa}) \in \Re^{6x7}.$  The
      matrix is not square, so does not have an inverse.  But having <i>more</i>
      degrees of freedom than the desired spatial velocity requires (more
      columns than rows) is actually the good case, in the sense that we might
      have many solutions for $v$ that can achieve a desired spatial velocity.
      To choose one of them (the minimum-norm solution), we can consider using
      the <a
      href="https://en.wikipedia.org/wiki/Moore%E2%80%93Penrose_inverse">Moore-Penrose
      pseudo-inverse</a>, $J^+$, instead: \begin{equation}v =
      [J^G(q)]^+V^{G_d}.\end{equation}</p>

      <p>The pseudo-inverse is a beautiful mathematical concept.  When the $J$
      is square and full-rank, the pseudo-inverse returns the true inverse of
      the system. When there are many solutions (here many joint velocities that
      accomplish the same end-effector spatial velocity), then it returns the
      minimum-norm solution (the joint velocities that produce the desired
      spatial velocity which are closest to zero in the least-squares sense).
      When there is no exact solution, it returns the joint velocities that
      produce an spatial velocity which is as close to the desired end-effector
      velocity as possible, again in the least-squares sense.  So good!</p>

      <example><h1>Our first end-effector "controller"</h1>
      
        <p>Let's write a simple controller using the pseudo-inverse of the
        Jacobian.  First, we'll write a new <code>LeafSystem</code> that defines
        one input port (for the iiwa measured position), and one output port
        (for the iiwa joint velocity command).  Inside that system, we'll ask
        MultibodyPlant for the gripper Jacobian, and compute the joint
        velocities that will implement a desired gripper spatial velocity.</p>

        <div>
          <script type="text/javascript">
          var pinv = { "name": "PseudoInverseController", 
            "input_ports" : ["iiwa_position"],
            "output_ports" : ["iiwa_velocity_command"]
            };
          document.write(system_html(pinv));
          </script>
        </div>

        <p>To keep things simple for this first example, we'll just command a
        constant gripper spatial velocity, and only run the simulation for a few
        seconds.</p>

        <p>Note that we do have to add one additional system into the diagram.
        The output of our controller is a desired joint velocity, but the input
        that the iiwa controller is expecting is a desired joint position.  So
        we will insert an integrator in between.</p>

        <p>I don't expect you to understand every line in this example, but it's
        worth finding the important lines and making sure you can change them
        and see what happens!</p>

        <p><a target="pick"
          href="https://colab.research.google.com/github/RussTedrake/manipulation/blob/master/pick.ipynb#scrollTo=6v-EGfoI3y6V"><img
          src="https://colab.research.google.com/assets/colab-badge.svg" alt="Open In Colab"/></a>
          </p>
  
        <p>Congratulations!  Things are really moving now.</p>

      </example>

    </subsection>

    <subsection><h1>Invertibility of the Jacobian</h1>

      <p>There is a simple check to understand when the pseudo-inverse can give
      an exact solution for any spatial velocity (achieving exactly the desired
      spatial velocity): the Jacobian must be full <i>row</i>-rank.  In this
      case, we need $\rank(J) = 6$. But assigning an integer rank to a matrix
      doesn't tell the entire story; for a real robot with (noisy)
      floating-point joint positions, as the matrix gets <i>close</i> to losing
      rank, (pseudo-)inverse starts to "blow-up".  A better metric, then, is to
      watch the smallest <a
      href="https://en.wikipedia.org/wiki/Singular_value_decomposition">singular
      value</a>; as this approaches zero, the norm of the pseudo-inverse will
      approach infinity.</p>

      <example><h1>Invertibility of the gripper Jacobian</h1>
      
        <p>You might have noticed that I printed out the smallest singular value
        of $J^G$ in one of the previous examples.  Take it for another spin.
        See if you can find configurations where the smallest singular value
        gets close to zero.</p>

        <p><a target="pick"
          href="https://colab.research.google.com/github/RussTedrake/manipulation/blob/master/pick.ipynb#scrollTo=hSeD3PBotJUU"><img
          src="https://colab.research.google.com/assets/colab-badge.svg" alt="Open In Colab"/></a>
          </p>

        <p>Here's a hint: try some configurations where the arm is very
        straight, (e.g. driving joint 2 and 4 close to zero).</p>

        <p>Another good way to find the singularities are to use your
        pseudo-inverse controller to send gripper spatial velocity commands that
        drive the gripper to the limits of the robot's workspace.  Try it and
        see!  In fact, this is the common case, and one that we will work hard
        to avoid.</p>

      </example>

      <p>Configurations $q$ for which $\rank(J(q_{iiwa})) < 6$ for a frame of
      interest (like our gripper frame $G$) are called <i>kinematic
      singularities</i>.  Try to avoid going near them if you can!  The iiwa has
      many virtues, but admittedly its kinematic workspace is not one of them.
      Trust me, if you try to get a big Kuka to reach into a little kitchen sink
      all day, every day, then you will spend a non-trivial amount of time
      thinking about avoiding singularities.</p>

      <p>In practice, things can get a lot better if we stop bolting our robot
      base to a fixed location in the world.  Mobile bases add complexity, but
      they are wonderful for improving the kinematic workspace of a robot.</p>

      <example><h1>Are kinematic singularities real?</h1>

        <p>A natural question when discussing singularities is whether they are
        somehow real, or whether they are somehow an artifact of the analysis.
        Perhaps it is useful to look at an extremely simple case.</p>

        <p>Imagine a two-link arm, where each link has length one.  Then the
        kinematics reduce to $$p^G = \begin{bmatrix} c_0 + c_{0+1} \\ s_0 + s_{0
        + 1} \end{bmatrix},$$  where I've used the (very common) shorthard $s_0$
        for $\sin(q_0)$ and $s_{0+1}$ for $\sin(q_0+q_1)$, etc.  The Jacobian is
        $$J^G(q) = \begin{bmatrix} -s_0 - s_{0+1} & -s_{0 + 1} \\ c_0 + c_{0 +
        1} & c_{0 + 1} \end{bmatrix},$$ and as expected, it loses rank when the
        arm is at full extension (e.g. when $q_0 = q_1 = 0$ which implies the
        first row is zero).</p>

        <figure>
          <iframe style="border:0;height:300px;width:400px;" src="data/two_link_singularities.html"></iframe>
        </figure>

        <p>Let's move the robot along the $x$-axis, by taking $q_0(t) = 1-t,$
        and $q_1(t) = -2 + 2t$.  This clearly visits the singularity $q_0 = q_1
        = 0$ at time 1, and then leaves again without trouble.  In fact, it does
        all this with a <i>constant</i> joint velocity ($\dot{q}_0=-1,
        \dot{q}_1=2$)!  The resulting trajectory is $$p^G(t) = \begin{bmatrix}
        2\cos(1-t) \\ 0 \end{bmatrix}.$$</p>

        <p>There are a few things to understand.  At the singularity, there is
        nothing that the robot can do to move its gripper farther in positive
        $x$ -- that singularity is real.  But it is also true that there is no
        way for the robot to move instantaneously back in the direction of $-x.$
        The Jacobian analysis is not an approximation, it is a perfect
        description of the relationship between joint velocities and gripper
        velocities.  However, just because you cannot achieve an instantaneous
        velocity in the backwards direction, it does not mean you cannot get
        there!  At $t=1$, even though the joint velocities are constant, and the
        Jacobian is singular, the robot is <i>accelerating</i> in $-x$,
        $\ddot{p}^G_{W_x}(t) = -2\cos(1-t).$</p>

        <todo>Update this to use the two-link iiwa (like i've done in the qp_diff_ik notebook).  The link lengths aren't *quite* one to one, but it's close.  could have a point on the second link that is the right distance away, or just add the ratio logic here.</todo>

      </example>

    </subsection>

  </section>

  <section><h1>Defining the grasp and pre-grasp poses</h1>
  
    <p>I'm going to put my red foam brick on the table.  Its geometry is <a
    href="https://github.com/RobotLocomotion/drake/blob/master/examples/manipulation_station/models/061_foam_brick.sdf#L21">defined</a>
    as a 7.5cm x 5cm x 5cm box.  For reference, the distance between the fingers
    on our gripper in the default "open" position is 10.7cm.  The "palm" of the
    gripper is 3.625cm from the body origin, and the fingers are 8.2cm long.</p>
    
    <p>To make things easy to start, I'll promise to set the object down on the
    table with the object frame's $z$-axis pointing up (aligned with the world
    $z$ axis), and you can assume it is resting on the table safely within the
    workspace of the robot.  But I reserve the right to give the object
    arbitrary initial yaw.  Don't worry, you might have noticed that the
    seventh joint of the iiwa will let you rotate your gripper around quite
    nicely (well beyond what my human wrist can do).</p>
    
    <p>Observe that visually the box has rotational symmetry -- I could always
    rotate the box 90 degrees around its $x$-axis and you wouldn't be able to
    tell the difference.  We'll think about the consequences of that more in the
    next chapter when we start using perception.  But for now, we are ok using
    the omniscient "cheat port" from the simulator which gives us the
    unambiguous pose of the brick.</p>

    <figure>
      <todo>Update this with a figure that includes the object and object
      frame.</todo>
      <img style="width:40%" src="figures/kinematic_frames_gripper.png"/>
      <figcaption>The gripper frame and the object frame.  For each frame, the
      <span style="color:red">positive $x$ axis is in red</span>, the <span
      style="color:green">positive $y$ axis is in green</span>, and the <span
      style="color:blue">positive $z$ axis is in blue</span> (XYZ $\Leftrightarrow$
      RGB).</figcaption>
    </figure>

    <p>Take a careful look at the gripper frame in the figure above, using the
    colors to understand the axes.  Here is my thinking:  Given the size of the
    hand and the object, I want the <i>desired</i> position (in meters) of the
    object in the gripper frame, $${}^{G_{grasp}}p^O = \begin{bmatrix} 0 \\ 0.12
    \\ 0 \end{bmatrix}, \qquad {}^{G_{pregrasp}}p^O = \begin{bmatrix} 0 \\ 0.2
    \\ 0 \end{bmatrix}.$$  Recall that the logic behind a <i>pregrasp</i> pose
    is to first move to safely above the object, if our only gripper motion that
    is very close to the object is a straight translation from the pregrasp pose
    to the grasp pose and back, then it allows us to mostly avoid having to
    think about collisions (for now).  I want the orientation of my gripper to
    be set so that the positive $z$ axis of the object aligns with the negative
    $y$ axis of the gripper frame, and the positive $x$ axis of the object
    aligns with the positive $z$ axis of the gripper.  We can accomplish that
    with $${}^{G_{grasp}}R^O = \text{MakeXRotation}\left(\frac{\pi}{2}\right)
    \text{MakeZRotation} \left(\frac{\pi}{2}\right).$$  I admit I had my right
    hand in the air for that one!  Our pregrasp pose will have the same
    orientation as our grasp pose.</p>

    <example><h1>Computing grasp and pregrasp poses</h1>
    
      <figure>
        <img style="width:70%" src="figures/grasp_pose.png"/>
      </figure>
  
      <p>Here is a simple example of loading a floating Schunk gripper and a
      brick, computing the grasp / pregrasp pose (drawing out each
      transformation clearly), and rendering the hand relative to the
      object.</p>

      <p><a target="pick"
        href="https://colab.research.google.com/github/RussTedrake/manipulation/blob/master/pick.ipynb#scrollTo=QuFmomDOxoAR"><img
        src="https://colab.research.google.com/assets/colab-badge.svg" alt="Open In Colab"/></a>
        </p>

      <p>I hope you can see the value of having good notation at work here.  My
      right hand was in the air when I was deciding what a suitable relative
      pose for the object in the hand should be (when writing the notes).  But
      once that was decided, I went to type it in and everything just
      worked.</p>

    </example>
  
  </section>

  <section><h1>A pick and place trajectory</h1>
  
    <p>We're getting close.  We know how to produce desired gripper poses, and
    we know how to change the gripper pose instantaneously using spatial
    velocity commands. Now we need to specify how we want the gripper poses to
    change over time, so that we can convert our gripper poses into spatial
    velocity commands.</p>

    <p>Let's define all of the "keyframe" poses that we'd like the gripper to
    travel through, and time that it should visit each one.  The following colab
    example does precisely that.</p>

    <example><h1>A plan "sketch"</h1>

      <p><a target="pick"
        href="https://colab.research.google.com/github/RussTedrake/manipulation/blob/master/pick.ipynb#scrollTo=zYuRDF6mSBx5"><img
        src="https://colab.research.google.com/assets/colab-badge.svg" alt="Open In Colab"/></a>
        </p>

      <figure>
        <img style="width:70%" src="figures/pick_sketch.png"/>
        <figcaption>Keyframes of the gripper.  The robot's base will be at the
        origin, so we're looking over the (invisible) robot's shoulder here. The
        hand starts in the "initial" pose near the center, moves to the
        "prepick" to "pick" to "prepick" to "clearance" to "preplace" to "place"
        and finally back to "preplace".</figcaption>
      </figure>

      <todo>timeline graphic here, from time zero, to pre-grasp, to grasp, to
      </todo>

      <p>How did I choose the times?  I started everything at time, $t=0$, and
      listed the rest of our times as absolute (time from zero). That's when the
      robot wakes up and sees the brick.  How long should we take to transition
      from the starting pose to the pregrasp pose?  A really good answer might
      depend on the exact joint speed limits of the robot, but we're not trying
      to move fast yet.  Instead I chose a conservative time that is
      proportional to the total Euclidean distance that the hand will travel,
      say $k=10~s/m$ (aka $10~cm/s$): $$t_{pregrasp} = k
      \left\|{}^{G_0}p^{G_{pregrasp}}\right\|_2.$$  I just chose a fixed
      duration of 2 seconds for the transitions from pregrasp to grasp and back,
      and also left 2 seconds with the gripper stationary for the segments where
      the fingers needs to open/close.</p>
  
    </example>

    <div>To fill in the details -- defining a gripper pose for all $t$ -- we'll
    convert these keyframes into a proper <i>trajectory</i>, $X^G(t), \forall t
    \in [t_{initial}, t_{postpick}].$ Actually, we will produce a handful of
    trajectories, one for each of the:
      <ul>
        <li>desired gripper positions,</li>
        <li>desired gripper orientations, and</li>
        <li>desired gripper finger positions (open and close).</li>
      </div>
  
    <p>There are a number of ways one might represent a trajectory
    (mathematically or computationally).  We have a pretty good collection of
    <a
    href="https://drake.mit.edu/doxygen_cxx/namespacedrake_1_1trajectories.html">trajectory</a>
    classes available in Drake.  Many of them are implemented as <a
    href="https://en.wikipedia.org/wiki/Spline_(mathematics)">splines</a> --
    piecewise polynomial functions of time.  Interpolating between orientations
    requires some care, but for positions we can do a simple linear
    interpolation between each of our keyframes.  That would be called a "<a
    href="https://en.wikipedia.org/wiki/First-order_hold">first-order hold</a>",
    and it's implemented in Drake's <a
    href="https://drake.mit.edu/doxygen_cxx/classdrake_1_1trajectories_1_1_piecewise_polynomial.html">PiecewisePolynomial</a>
    class.  For rotations, we'll use something called "spherical linear
    interpolation" or <a href="https://en.wikipedia.org/wiki/Slerp">slerp</a>,
    which is implemented in Drake's <a
    href="https://drake.mit.edu/doxygen_cxx/classdrake_1_1trajectories_1_1_piecewise_quaternion_slerp.html">PiecewiseQuaternionSlerp</a>.</p>

    <example><h1>Grasping with trajectories</h1>
    
      <p>There are a number of ways to visualize the trajectory when it's
      connected to 3D.  I've plotted the <i>position</i> trajectory as a
      function of time below.<p>  

      <p><a target="pick"
        href="https://colab.research.google.com/github/RussTedrake/manipulation/blob/master/pick.ipynb#scrollTo=zYuRDF6mSBx5"><img
        src="https://colab.research.google.com/assets/colab-badge.svg" alt="Open In Colab"/></a>
        </p>

      <figure>
        <img width="60%" src="data/gripper_position_trajectory.svg"/>
<!--        <iframe scrolling="no" style="border:none;" seamless="seamless"
        height=400 width="100%" src="data/gripper_position_trajectory.html"/> -->
      </figure>

      <p></p>With 3D data, you can <a
      href="data/gripper_position_trajectory.html">plot it in 3D</a>.  But my
      favorite approach is as <a href="data/gripper_trajectory.html">an
      animation in our 3D renderer</a>!  Make sure you try the "Open
      controls>Animation" interface.  You can pause it and then scrub through
      the trajectory using the time slider.</p>

      <p>For a super interesting discussion on how we might visualize the 4D
      quaternions as creatures trapped in 3D, you might enjoy <a
      href="https://eater.net/quaternions">this series of "explorable"
      videos</a>.</p>
    </example>

  </section>

  <section><h1>Putting it all together</h1>
  
    <p>We can slightly generalize our PseudoInverseController to have additional
    input ports for the gripper position and gripper orientation (in our first
    version, the spatial velocity command was just hard-coded in the
    controller).</p>

    <div>
      <script type="text/javascript">
      var pinv = { "name": "PseudoInverseController", 
        "input_ports" : ["omega_WG", "v_WG", "iiwa_position"],
        "output_ports" : ["iiwa_velocity_command"]
        };
      document.write(system_html(pinv));
      </script>
    </div>

    <p>So far, our trajectories are position and orientation trajectories.  Our
    controller needs translational velocity and angular velocity commands.
    Fortunately, the trajectory classes we have used support differentiating the
    trajectories.  In fact, the <code>PiecewiseQuaternionSlerp</code> is clever
    enough to return the derivative of the 4-component quaternion trajectory as
    a 3-component angular velocity trajectory.  The rest is just a matter of
    wiring up the system diagram.</p>

    <example><h1>The full pick and place demo</h1>
    
      <p>The next few cells of the notebook should get you a pretty satisfying
      result.  <a href="data/pick.html">Click here to watch it without doing the
      work</a>.</p>

      <p><a target="pick"
        href="https://colab.research.google.com/github/RussTedrake/manipulation/blob/master/pick.ipynb#scrollTo=AeKMhIacWZ00"><img
        src="https://colab.research.google.com/assets/colab-badge.svg" alt="Open In Colab"/></a>
        </p>

      <figure>
        <img width="100%" src="data/pick_diagram.svg"/>
      </figure>

      <p>It's worth scrutinizing the result.  For instance, if you examine the
      context at the final time, how close did it come to the desired final
      gripper position?  Are you happy with the joint positions?  If you ran
      that same trajectory in reverse, then back and forth (as an industrial
      robot might), would you expect errors to accumulate?</p>

    </example>

  </section>

  <section id="diff_ik_w_constraints"><h1>Differential inverse kinematics with
  constraints</h1>

    <p>Our solution above works in many cases.  We could potentially move on.
    But with just a little more work, we can get a much more robust solution...
    one that we will be happy with for many chapters to come.</p>

    <p>So what's wrong with the pseudo-inverse controller?  You won't be
    surprised when I say that it does not perform well around singularities.
    When the minimum singular value of the Jacobian gets small, that means that
    some values in the inverse get very large.  If you ask our controller to
    track a seemingly reasonable end-effector spatial velocity, then you might
    have extremely large velocity commands that result.</p>

    <p>There are other important limitations, though, which are perhaps more
    subtle. The real robot has constraints, very real constraints on the joint
    angles, velocities, accelerations, and torques.  If you, for instance, send
    a velocity command to the iiwa controller that cannot be followed, that
    velocity will be clipped.  In the mode we are running the iiwa
    (joint-impedance mode), the iiwa doesn't know anything about your
    end-effector goals.  So it will very likely simply saturate your velocity
    commands independently joint by joint.  The result, I'm afraid, will not be
    as convenient as a slower end-effector trajectory.  Your end-effector could
    run wildly off course.</p>

    <p>Since we know the limits of the iiwa, a better approach is to take these
    constraints into account at the level of our controller.  It's relatively
    straight-forward to take position, velocity, and acceleration constraints
    into account; torques would require a full dynamics model so we won't worry
    about them yet here.</p>

    <subsection><h1>Pseudo-inverse as an optimization</h1>

      <p>I introduced the pseudo-inverse as having almost magical properties: it
      returns an exact solution when one is available, or the best possible
      solution (in the least-squares norm) when one is not.  These properties
      can all be understood by realizing that the pseudo-inverse is just the
      optimal solution to a least-squares optimization problem: \begin{equation}
      \min_v \left|J^G(q)v - V^{G_d}\right|^2_2. \end{equation}  When I write an
      optimization of this form, I will refer to $v$ as the decision
      variable(s), and I will use $v^*$ to denote the optimal solution (the
      value of the decision variables that minimizes the cost).  Here we have
      $$v^* = \left[ J^G(q) \right]^+V^{G_d}.$$</p>

      <p>Optimization is an incredibly rich topic, and we will put many tools
      from optimization theory to use over the course of this text.  For a
      beautiful rigorous but accessible treatment of convex optimization, I
      highly recommend <elib>Boyd04a</elib>; it is free online and even reading
      the first chapter can be incredibly valuable.  For a very short
      introduction to using optimization in Drake, please take a look at the
      tutorials on "Solving Mathematical Programs" linked from the Drake <a
      href="http://drake.mit.edu">front page</a>.  I use the term "mathematical
      program" almost synonymously with "optimization problem".  Mathematical
      program is slightly more appropriate if we don't actually have an
      objective; only constraints.</p>
    
    </subsection>

    <subsection><h1>Adding velocity constraints</h1>
    
      <p>Once we understand our existing solution through the lens of
      optimization, we have a natural route to generalizing our approach to
      explicitly reason about the constraints.  The velocity constraints are the
      most straight-forward to add.  \begin{align} \min_v && \left|J^G(q)v -
      V^{G_d}\right|^2_2, \\ \subjto && v_{min} \le v \le v_{max}. \nonumber
      \end{align}  You can read this as "find me the joint velocities that
      achieve my desired gripper spatial velocity as closely as possible, but
      satisfy my joint velocity constraints."  The solution to this can be much
      better than what you would get from solving the unconstrained optimization
      and then simply trimming any velocities to respect the constraints after
      the fact.</p>

      <p>This is, admittedly, a harder problem to solve in general.  The
      solution cannot be described using only the pseudo-inverse of the
      Jacobian.  Rather, we are going to solve this (small) optimization problem
      directly in our controller every time it is evaluated.  This problem has a
      convex quadratic objective and linear constraints, so it falls into the
      class of convex Quadratic Programming (QP).  This is a particularly nice
      class of optimization problems where we have very strong numerical
      tools.</p>

      <example><h1>Jacobian-based control with velocity constraints</h1></example>
    
    </subsection>

    <subsection><h1>Adding position and acceleration constraints</h1>

      <p>We can easily add more constraints to our QP, without significantly
      increasing the complexity, as long as they are linear in the decision
      variables.  So how should we add constraints on the joint position and
      acceleration?</p>        

      <p>The natural approach is to make a first-order approximation of these
      constraints.  To do that, the controller needs some characteristic time step / timescale to relate its velocity decisions to positions and accelerations.  We'll denote that time step as $h$.</p>
      
      <p>The controller already accepts the current measured joint positions $q$
      as an input; let us now also take the current measured joint velocities
      $v$ as a second input.  And we'll use $v_n$ for our decision variable --
      the next velocity to command.  Using a simple <a
      href="https://en.wikipedia.org/wiki/Euler_method">Euler approximation</a>
      of position and first-order derivative for acceleration gives us the
      following optimization problem: \begin{align} \min_{v_n} \quad &
      \left|J^G(q)v_n - V^{G_d}\right|^2_2, \\ \subjto \quad & v_{min} \le v_n
      \le v_{max}, \nonumber \\ & q_{min} \le q + h v_n \le q_{max}, \nonumber
      \\ & \dot{v}_{min} \le \frac{v_n - v}{h} \le \dot{v}_{max}. \nonumber
      \end{align}</p>

    </subsection>

    <subsection id="joint_centering"><h1>Joint centering</h1>
  
      <p><i>(Need to finish/clean up the writing here)</i>.  Our Jacobian is $6
      \times 7$.  We have extra degrees of freedom.  Not only an opportunity,
      but a responsibility.  If we don't tell them what we want them to do, then
      you might not like what you get.  Discuss analytic centering.  Add a term
      to the objective in the nullspace of our current objective.</p>

      <p>Here is one way to formulate it.  If we had no end-effector goals, then
      we might write the controller $v = K(q_0 - q)$, a proportional control
      that would drive the robot to its nominal configuration.  In the
      unconstrained control case, there is a very elegant way of implementing
      this controller as a "secondary" control, guaranteed not to interfere with
      our primary end-effector spatial velocity controller, by projecting it
      into the nullspace of $J^G$.  We can use almost the same trick here.</p>
      
      <p>Denote $P(q)$ as an orthonormal basis for the kernel of a Jacobian $J$.
      Traditionally in robotics we implemented this using the pseudo-inverse: $P
      = (I - J^+J)$, but many linear algebra packages now provide methods to
      obtain one more directly.  Adding $Pv \approx Pk(q_0 - q)$ as a secondary
      objective can be accomplished with \begin{align}\min_{v_n} \quad &
      \left|J^G(q)v_n - V^{G_d}\right|^2_2 + \epsilon \left|P(q)\left(v_n -
      K(q_0 - q)\right)\right|^2_2, \\ \subjto \quad &
      \text{constraints}.\nonumber\end{align} Note the scalar $\epsilon$ that
      we've placed in front of the secondary objective.  There is something
      important to understand here.  If we do not have any constraints, then we
      can remove $\epsilon$ completely -- the secondary task will in no way
      interfere with the primary task of tracking the spatial velocity. However,
      if there are constraints, then these constraints can cause the two
      objectives to clash (<a href="#competing_objectives">Exercise</a>).  So we pick $\epsilon \ll 1$ to give the primary
      objective relatively more weight.  But don't make it too small, because
      that might make the numerics bad for your solver.  I'd say $\epsilon ~=
      0.01$ is just about right.</p>

      <p>There are more sophisticated methods if one wishes to establish a
      strict task prioritization in the presence of constraints (e.g.
      <elib>Flacco15+Escande14</elib>), but for th simple prioritization we have
      formulated here, the penalty method is quite reasonable.</p>

    </subsection>

    <subsection><h1>Alternative formulations</h1>
    
      <p>Once we have embraced the idea of solving a small optimization problem
      in our control loop, many other formulations are possible, too.  You will
      find many in the literature.  Minimizing the least-squares distance
      between the commanded spatial velocity and the resulting velocity might
      not actually be the best solution.  The formulation we have been using
      heavily in Drake adds an additional constraint that our solution
      <b>must</b> move in the same <i>direction</i> as the commanded spatial
      velocity.  If we are up against constraints, then we may slow down, but we
      will not deviate (instantaneously) from the commanded path.  It would be a
      pity to spend a long time carefully planning collision-free paths for your
      end-effector, only to have your controller treat the path as merely a
      suggestion.  Note, however, that your plan playback system still needs to be smart enough to realize that the slow-down occurred (open-loop velocity commands are not enough).</p>

      <p>\begin{align} \max_{v_n, \alpha} \quad & \alpha, \\ \subjto \quad &
      J^G(q)v_n = \alpha V^{G_d}, \nonumber \\ & 0 \le \alpha \le 1, \nonumber
      \\ & \text{additional constraints}. \nonumber \end{align}  You should
      immediately ask yourself: is it reasonable to scale a spatial velocity by
      a scalar?  That's another great <a
      href="#scaling_spatial_velocity">exercise</a>. </p>

      <todo>Updated block diagram</todo>

      <example><h1>Drake's DifferentialInverseKinematics</h1>
      
      </example>
       
      <p>We will use this implementation of differential inverse kinematics
      whenever we need to command the end-effector in the next few chapters.</p>

      <todo>We could add collision-avoidance constraints naturally here, too.
      But I haven't introduced those ideas yet, so instead I should link forward
      to the relevant exposition once it exists.</todo>

    </subsection>

    <todo>Find a home for inverse kinematics, including nonlinear optimization
    and closed-form solutions (e.g. IK-fast)</todo>

  </section>

  <section><h1>Exercises</h1>
  
  <exercise><h1>Spatial frames and positions.</h1>
  
    <figure><img style="width:60%" src="data/grasp_frames.png"/></figure>

    <div>I've rendered the gripper and the brick with their corresponding body
    frames.  Given the configuration displayed in the figure, which is a
    possible value for ${^Gp^O}$?
    <ol type="a">
      <li>[0.2, 0, -.2]</li> <!-- p_GO_W -->
      <li>[0, 0.3, .1]</li>  <!-- p_GO_G -->
      <li>[0, -.3, .1]</li>  <!-- p_OG_O -->
    </ol>
    Which is a possible value for ${^Gp^O_W}$?
    </div>
  
  </exercise>
  
  <exercise id="angular_velocity_addition"><h1>The additive property of angular
  velocity vectors.</h1>
  
    <p>TODO(russt): Fill this in.</p>

  </exercise>

  <exercise id="slerp"><h1>Spherical linear interpolation (slerp)</h1>
  
  </exercise>
  
  <exercise id="scaling_spatial_velocity"><h1>Scaling spatial velocity</h1>
  
    <p>TODO(russt): Fill this in.  Until then, here's a little code that might convince you it's reasonable.</p>

    <pysrcinclude>figures/scaling_spatial_velocity.py</pysrcinclude>
  
  </exercise>

  <exercise id="planar_manipulator"><h1>Planar Manipulator</h1>
      <p> For this exercise you will derive the translational forward kinematics ${^A}p{^C}=f(q)$ and the translational Jacobian $J(q)$ of a planar two-link manipulator. You will work exclusively in <a href="https://colab.research.google.com/github/RussTedrake/manipulation/blob/master/exercises/pick/planar_manipulator.ipynb" target="_blank">this notebook</a>. You will be asked to complete the following steps: </p>

      <ol type="a">
        <li> Derive the forward kinematics of the manipulator. </li>
        <li> Derive the Jacobian matrix of the manipulator. </li>
        <li> Analyze the kinematic singularities of the manipulator from the Jacobian.</li>
      </ol>
  </exercise>

  <exercise id="spatial_transforms"><h1>Spatial Transforms and Grasp Pose</h1>
      <p> For this exercise you will apply your knowledge on spatial algebra to write poses of frames in different reference frames, and design a grasp pose yourself. You will work exclusively in <a href="https://colab.research.google.com/github/RussTedrake/manipulation/blob/master/exercises/pick/rigid_transforms.ipynb" target="_blank">this notebook</a>. You will be asked to complete the following steps: </p>

      <ol type="a">
        <li> Express poses of frames in different reference frames using spatial algebra. </li>
        <li> Design grasp poses given the configuration of the target object and griper configuration. </li>
      </ol>
  </exercise>

  <exercise id="robot_painter"><h1>The Robot Painter</h1>
      <p> For this exercise you will design interesting trajectories for the robot to follow, and observe the robot virtually painting in the air! You will work exclusively in <a href="https://colab.research.google.com/github/RussTedrake/manipulation/blob/master/exercises/pick/robot_painter.ipynb" target="_blank">this notebook</a>. You will be asked to complete the following steps: </p>

      <ol type="a">
        <li> Design and compute the poses of key frames of a designated trajectory. </li>
        <li> Construct trajectories by interpolating through the key frames. </li>
      </ol>
  </exercise>  

  <exercise id="intro_to_qp"><h1>Introduction to QPs</h1>
      <p> For this exercise you will practice the syntax of solving Quadratic Programs (QPs) via Drake's MathematicalProgram interface. You will work exclusively in <a href="https://colab.research.google.com/github/RussTedrake/manipulation/blob/master/exercises/pick/intro_to_qp.ipynb" target="_blank">this notebook</a>.</p> 
  </exercise>  

  <exercise id="virtual_wall"><h1>Virtual Wall</h1>
      <p> For this exercise you will implement a virtual wall for a robot manipulator, using an optimization-based approach to differential inverse kinematics. You will work exclusively in <a href="https://colab.research.google.com/github/RussTedrake/manipulation/blob/master/exercises/pick/differential_ik_optimization.ipynb" target="_blank">this notebook</a>. You will be asked to complete the following steps: </p>

      <ol type="a">
        <li> Implement a optimization-based differential IK controller with joint velocity limits. </li>
        <li> Using your own constraints, implement a virtual wall in the end-effector space using optimization-based differential IK controller. </li>
      </ol>
  </exercise>  

  <exercise id="competing_objectives"><h1>Competing objectives</h1>

    <p>In the section on <a href="#joint_centering">joint centering</a>, I
    claimed that an secondary objective might compete with a primary objective
    if they are linked through constraints.  To see this, consider the following
    optimization problem \begin{align*}\min_{x,y} \quad (x-5)^2 +
    (y+3)^2.\end{align*}  Clearly, the optimal solution is given by $x^*=5,
    y^*=-3$.  Moreover, the objective are separable.  The addition of the second
    objective, $(y+3)^2$, did not in any way impact the solution of the
    first.</p>
    
    <p>Now consider the constrained optimization \begin{align*}\min_{x,y} \quad&
    (x-5)^2 + (y+3)^2 \\ \subjto \quad& x - y \le 6. \end{align*} If the second
    objective was removed, then we would still have $x^*=5$.  What is the result
    of the optimization as written (it's only a few lines of code, if you want
    to do it that way)?  <!-- answer: 4, -2 --> I think you'll find that these
    "orthogonal" objectives actually compete!</p>

    <p>What happens if you change the problem to \begin{align*}\min_{x,y} \quad&
    (x-5)^2 + \frac{1}{100}(y+3)^2 \\ \subjto \quad& x - y \le 6? \end{align*}
    I think you'll find that solution is quite close to $x^*=5$, but also that
    $y^*$ is quite different than that $y^*=0$ one would obtain if the
    "secondary" objective was omitted completely.</p>

    <p>Note that if the constraint was not active at the optimal solution (e.g.
    $x=5, y=-3$ satisfied the constraints), then the objectives do not
    compete.</p>

    <p>This seems like an overly simple example.  But I think you'll find that
    it is actually quite similar to what is happening in the nullspace objective
    formulation above.</p>

  </exercise>

  </section>

</chapter>
<!-- EVERYTHING BELOW THIS LINE IS OVERWRITTEN BY THE INSTALL SCRIPT -->

<table style="width:100%;"><tr style="width:100%">
  <td style="width:33%;text-align:left;"><a class="previous_chapter" href=robot.html>Previous Chapter</a></td>
  <td style="width:33%;text-align:center;"><a href=index.html>Table of contents</a></td>
  <td style="width:33%;text-align:right;"><a class="next_chapter" href=pose.html>Next Chapter</a></td>
</tr></table>

<div id="footer">
  <hr>
  <table style="width:100%;">
    <tr><td><a href="https://accessibility.mit.edu/">Accessibility</a></td><td style="text-align:right">&copy; Russ
      Tedrake, 2020</td></tr>
  </table>
</div>


</body>
</html>
